/*
 * Kalman.h
 *
 *  Created on: Oct 13, 2012
 *      Author: HUNG
 */

#ifndef KALMAN_H_
#define KALMAN_H_
#include "Matrix.h"
#include "RoutingDef.h"
/*
 *
 */
class Kalman
{
    private:
        Matrix X, X0;
        Matrix A, B, U, Q;
        Matrix H, R;
        Matrix P, P0;
        Matrix Z_Pre; //Z^
        double dt; //time step
        double processNoiseIntensity; //Intensity of Process noise
        double measurementNoiseIntensity; //Intensity of Measurement noise
    public:
        Kalman();
        Kalman(double TimeStep, double ProcessNoiseIntensity, double MeasurementNoiseIntensity);
        virtual ~Kalman();
        void Predict();
        void Update(Matrix Z);

        void setX(Matrix _X)
        {
            this->X = _X;
        }
        Matrix getX()
        {
            return this->X;
        }
        Matrix getX0()
        {
            return this->X0;
        }
        void setP(Matrix _P)
        {
            this->P = _P;
        }
        Matrix getP()
        {
            return this->P;
        }
        Matrix getP0()
        {
            return this->P0;
        }
        void setA(Matrix _A)
        {
            this->A = _A;
        }
        void setB(Matrix _B)
        {
            this->B = _B;
        }
        void setU(Matrix _U)
        {
            this->U = _U;
        }
        void setQ(Matrix _Q)
        {
            this->Q = _Q;
        }
        void setH(Matrix _H)
        {
            this->H = _H;
        }
        void setR(Matrix _R)
        {
            this->R = _R;
        }
        void setZ_Pre(Matrix _Z)
        {
            this->Z_Pre = _Z;
        }

};

#endif /* KALMAN_H_ */
